
/**
 * @file SolverDelta.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/08/07
//  create a general solver for multi-gpss ppp solution
//  shjzhang
//
//  2020/01/16
//  remove stateMap/covMap, which makes the program complicated
//  shjzhang.
//
//============================================================================


#include "SolverDelta.hpp"
#include "SystemTime.hpp"
#include "Ambiguity.hpp"
#include "NEUUtil.hpp"

#define debug 1

using namespace gpstk::StringUtils;

namespace gpstk
{

      // Index initially assigned to this class
   int SolverDelta::classIndex = 9600000;

      // Returns an index identifying this object.
   int SolverDelta::getIndex() const
   { return index; }

      // Returns a string identifying this object.
   std::string SolverDelta::getClassName() const
   { return "SolverDelta"; }

   void SolverDelta::Init(void)
   {
       // set up variables and equations for PPP

       // Warning: not consider the GLONASS system until now
       // Warning: only consider LC combinations
       // Warning: see SolverDeltaUC for UC combinations

       //////////////////////////////////////////////////////////////
       //
       // please augument for GAL/BDS system
       // 
       // wavelengthBLC12 = clight/(f1+f2)
       //
       // BLC12  = clight/(f1+f2)[( B1 + f2/(f1-f2)*Nw]
       // BLC15  = clight/(f1+f5)[( B1 + f5/(f1-f5)*Nw)]
       //
       //////////////////////////////////////////////////////////////

   }

   void SolverDelta::setUpEquations(void)
   {
       
       cout << "SolverDelta: setUpEquations" << endl;

       ///////////////////////////////////////////////
       //
       // Now, define variables need to be estimated 
       // 
       ///////////////////////////////////////////////
       
          // These variables are, by default, SourceID-indexed
       Variable cdt(       TypeID::dcdtSta, &wnModel);
       Variable cdtDelta(  TypeID::dcdtStaDelta, &wnModel);
       Variable tropo(     TypeID::wetMap,  &tropoModel );
       Variable deltaTropo(TypeID::deltaWetTropo,  &tropoModel );

       //
       // Define Equation object for code/phase observations, and add variables 
       //

       // GPS PC/LC
       Equation equPC12GPS( TypeID::prefitC12 );
       Equation equLC12GPS( TypeID::diffPrefitL12 );

       // GAL PC/LC
       Equation equPC15GAL( TypeID::prefitC15, SatelliteSystem::Galileo );
       Equation equLC15GAL( TypeID::diffPrefitL15, SatelliteSystem::Galileo );

       // BDS PC/LC                                        
       Equation equPC27BDS( TypeID::prefitC27, SatelliteSystem::BDS );
       Equation equLC27BDS( TypeID::diffPrefitL27, SatelliteSystem::BDS );

       //////////////////////////
       // insert variables now
       //////////////////////////
       // Insert cdt for datum system and isb for other systems
       equPC12GPS.addVariable(cdt, 1.0); 
       equPC12GPS.addVariable(tropo);

       equLC12GPS.addVariable(cdtDelta, 1.0);  
       equLC12GPS.addVariable(tropo, Variable(TypeID::diffWetMap));
       if(estDeltaTropo)
       {
          equLC12GPS.addVariable(deltaTropo, Variable(TypeID::wetMap));
       }

       // define the weight for all combinations relative PC12GPS

          // Phase equations should have higher relative weight
       equLC12GPS.setWeight(10000.0);     // 100.0 * 100.0

          ///////////
          // SETTING THE RULES: SETUP EQUATION SYSTEM
          ///////////
       
       // YOU MUST clear the equation system firstly!!!!!
       // only current equations is feed to equSystem
       equSystem.clearEquations();
       
          // Add equation descriptions
       if(usingGPS)
       {
           equSystem.addEquation(equPC12GPS);
           equSystem.addEquation(equLC12GPS);
       }


   } // end of setUpEquations




      /* Returns a reference to a gnssRinex object after solving
       *  the previously defined equation system.
       *
       * @param gData    Data object holding the data.
       */
   void SolverDelta::Process(commonHeader& comHeader, gnssRinex& gData)
       noexcept(false)
   {

      try
      {

          preCompute( gData );
          Compute( gData );
          postCompute( gData );
      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );
         GPSTK_THROW(e);
      }

      return;

   }  // End of method 'SolverDelta::Prepare()'



      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverDelta::preCompute( gnssRinex& gRin)
       noexcept(false)
   {
      try
      {
         if(debug)
         {
             cout << "epoch:" << YDSTime(gRin.header.epoch) << endl;

             cout << "begin of SolverDelta:" << gRin.header.source << endl;
             std::list<Equation> descripEquList;
             descripEquList = equSystem.getDescripEqus();
             cout << "size:" << descripEquList.size() << endl;
             for(std::list<Equation>::iterator it=descripEquList.begin();
                     it!=descripEquList.end();
                     ++it)
             {
                 (*it).dump(cout);
             }

             // dump
             gRin.dump(cout, 1);
         }

            // Prepare the equation system with current data
         equSystem.Prepare(gRin);


            // Get the number of unknowns being processed
         int numUnknowns( equSystem.getTotalNumVariables() );

            // Get the set with unknowns being processed
         currentUnkSet =  equSystem.getVarUnknowns() ;

         if(debug)
         {
             std::list<Equation> currentEquList;
             currentEquList = equSystem.getCurrentEquationsList();
             for(std::list<Equation>::iterator it=currentEquList.begin();
                     it!=currentEquList.end();
                     ++it)
             {
                 (*it).dump(cout);
             }

             cout << "currentUnkSet are as follows:" << endl;
             for(VariableSet::iterator vIt=currentUnkSet.begin();
                     vIt!=currentUnkSet.end();
                     vIt++)
             {
                 cout << (*vIt) << endl;
             }
         }

         // reset filter
         CommonTime epoch( gRin.header.epoch );

            // Feed the filter with the correct state and covariance matrix
         if( firstTime )
         {

            Vector<double> initialState(numUnknowns, 0.0);
            Matrix<double> initialCov( numUnknowns, numUnknowns, 0.0 );

               // Fill the initialCov matrix
            for( auto v: currentUnkSet )
            {
                int index = v.getNowIndex();
                initialCov(index,index) = v.getInitialVariance();
            }

               // Reset Kalman filter state and covariance matrix
            xhat = initialState; 
            P    = initialCov;

               // No longer first time
            firstTime = false;
         }
         else
         {

               // Adapt the size to the current number of unknowns
            Vector<double> currentState(numUnknowns, 0.0);
            Matrix<double> currentCov(numUnknowns, numUnknowns, 0.0);


               // Fill the state vector
            for( auto v: currentUnkSet )
            {
                int now_index = v.getNowIndex();
                int old_index = v.getPreIndex();
                if( old_index != -1)
                {
                    currentState(now_index) = solution( old_index );
                }
            }

               // Fill the covariance matrix, 2019/08/01, shjzhang
               // modify the code, 2020/01/16, shjzhang

               // We need a copy of 'currentUnkSet'
            VariableSet tempSet(currentUnkSet);
            for( auto v: currentUnkSet ) 
            {
                int nowIndex = v.getNowIndex();
                int oldIndex = v.getPreIndex();

                // old Variable
                if( oldIndex != -1 )
                {
                    for( auto v2: tempSet )
                    {
                        int nowIndex2 = v2.getNowIndex();
                        int oldIndex2 = v2.getPreIndex();

                        // old Variable
                        if( oldIndex2 != -1 )
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = covMatrix(oldIndex, oldIndex2);
                        }
                        else // new variable
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = 0.0;
                        }
                    }
                }
                else // new variable
                {
                    // only diagnol element is assigned to the new variable
                    currentCov(nowIndex, nowIndex) = v.getInitialVariance();
                }

                // erase var
                tempSet.erase(v);

            }  // End of for( VariableSet::const_iterator itVar1 = currentUnkSet...'

               // Reset Kalman filter to current state and covariance matrix
            xhat = currentState;
            P    = currentCov;

         }  // End of 'if(firstTime)'

            // State Transition Matrix (PhiMatrix)
         phiMatrix = equSystem.getPhiMatrix();
         qMatrix = equSystem.getQMatrix();

            // Get matrices and vectors out of equation system
            // Measurements vector (Prefit-residuals)
         measVector = equSystem.getPrefitsVector();
         hMatrix = equSystem.getGeometryMatrix();
         rMatrix = equSystem.getWeightsMatrix();

         if(debug)
         {
             cout << "measVector" << endl;
             cout << measVector << endl;
             cout << hMatrix<< endl;
             cout << rMatrix<< endl;
         }

         if(debug)
         {
            cout << "phiMatrix" << endl;
            cout <<  phiMatrix  << endl;

            cout << "qMatrix" << endl;
            cout <<  qMatrix  << endl;
         }

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverDelta::preCompute()'



      // Compute the solution of the given equations set.
      //
      // @param gData    Data object holding the data.
      //
      // \warning A typical kalman filter will be devided into 
      // two different phase, the prediction and the correction.
      // for some special application, for example, the 'integer'
      // satellite and receiver clock/bias estimation, the ambiguity
      // constraints are generated using the 'predicted' ambiguity 
      // values and its variances. 
      //
   gnssRinex& SolverDelta::Compute( gnssRinex& gRin )
       noexcept(false)
   {
      TimeUpdate( phiMatrix, qMatrix );
      MeasUpdate( measVector, hMatrix, rMatrix );

         // Return  
      return gRin;

   }  // End of method 'SolverDelta::Compute()'


      // Predict the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SolverDelta::TimeUpdate( const Matrix<double>& phiMatrix,
                                 const Matrix<double>& processNoiseCovariance )
       noexcept(false)
   {

         // Check whether the phiMatrix is square
      if (!(phiMatrix.isSquare()))
      {
         InvalidSolver e("phiMatrix is not square");
         GPSTK_THROW(e);
      }

         // Get the number of unknowns being processed
      int numUnknowns( equSystem.getTotalNumVariables() );
      int stateSize(xhat.size());
      if(!(stateSize==numUnknowns))
      {
         InvalidSolver e("TimeUpdate(): Size of a posteriori state estimation vector \
do not match the number of unknowns");
         GPSTK_THROW(e);
      }

      int phiRow = static_cast<int>(phiMatrix.rows());
      if (!(phiRow==numUnknowns))
      {
         InvalidSolver e("Number of unknowns does not match dimension \
of phiMatrix");
         GPSTK_THROW(e);
      }

      if (!(qMatrix.isSquare()))
      {
         InvalidSolver e("qMatrix is not square");
         GPSTK_THROW(e);
      }

      int qRow = static_cast<int>(qMatrix.rows());
      if (!(qRow==numUnknowns))
      {
         InvalidSolver e("Number of unknowns does not match dimension \
of qMatrix");
         GPSTK_THROW(e);
      }

      try
      {
            // Compute the a priori state vector
         xhatminus = phiMatrix*xhat;
         Matrix<double> phiT(transpose(phiMatrix));
            // Compute the a priori estimate error covariance matrix
         Pminus = phiMatrix*P*phiT + processNoiseCovariance;

      }
      catch(...)
      {
         InvalidSolver e("TimeUpdate(): Unable to predict next state.");
         GPSTK_THROW(e);
         return -1;
      }
        
      return 0;

   }  // End of method 'SolverDelta::Compute()'

      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SolverDelta::MeasUpdate( const Vector<double>& prefitResiduals,
                              const Matrix<double>& designMatrix,
                              const Matrix<double>& weightMatrix )
       noexcept(false)
   {

         // By default, results are invalid
      valid = false;
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("MeasUpdate(): Weight matrix is not square");
         GPSTK_THROW(e);
      }


      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("MeasUpdate(): prefitResiduals size does not match dimension of \
weightMatrix");
         GPSTK_THROW(e);
      }


      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("MeasUpdate(): prefitResiduals size does not match \
dimension of designMatrix");
         GPSTK_THROW(e);
      }


      int gCol = static_cast<int>(designMatrix.cols());
      int aprioriStateRow = static_cast<int>(xhatminus.size());
      if (!(gCol==aprioriStateRow))
      {
         InvalidSolver e("MeasUpdate(): designMatrix size and a priori state \
estimation vector do not match.");
         GPSTK_THROW(e);
      }


      int pMCol = static_cast<int>(Pminus.cols());
      if ( pMCol != aprioriStateRow )
      {
         InvalidSolver e("MeasUpdate(): Sizes of a priori error covariance \
matrix and a priori state estimation vector do not match.");
         GPSTK_THROW(e);
      }


         // After checking sizes, let's do the real correction work
      Matrix<double> invPMinus;
      Matrix<double> designMatrixT( transpose(designMatrix) );

      try
      {
         invPMinus = inverseChol(Pminus);
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute invPMinus matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
         Matrix<double> invTemp( designMatrixT * weightMatrix * designMatrix +
                                 invPMinus );
            // Compute the a posteriori error covariance matrix
         P = inverseChol( invTemp );
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
         xhat = P * ( (designMatrixT * weightMatrix * prefitResiduals) + 
                      (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

      xhatminus = xhat;
      Pminus = P;

      solution = xhat;
      covMatrix = P;

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

         // If everything is fine so far, then the results should be valid
      valid = true;

      return 0;

   }  // End of method 'SolverDelta::MeasUpdate()'



      /* Code to be executed after 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverDelta::postCompute( gnssRinex& gRin )
       noexcept(false)
   {
      try
      {
         CommonTime epoch(gRin.header.epoch);

         CivilTime cv(epoch);

         if(debug)
         {
             cout << "cv:" << cv << endl;
         }

         //
         // Save needed gnss rinex data for backward-processing
         //

         // Insert forward gnssRinex to forwardRinexData
         gnssRinex gBack(gRin);

         TypeIDSet typeSet;
         typeSet.insert(TypeID::wetMap);
         typeSet.insert(TypeID::diffWetMap);
         typeSet.insert(TypeID::weight);
         typeSet.insert(TypeID::satArc);

         if(usingGPS)
         {
             typeSet.insert(TypeID::prefitC12);
             typeSet.insert(TypeID::diffPrefitL12);
         }

         // to be extended

         // keep needed observation data for backward processing
         gBack.keepOnlyTypeID(typeSet);

            // save forward-processing data
         if(forwardProcessing)
         {
             if(debug)
             {
                 cout << pFloatSolStream << endl;
                 cout << "solution" << endl;
             }

             int numSat = gRin.numSats();

             // let's update the cdt using last epoch's cdt and cdtDelta
             // and current epoch's cdt
             double cdt    = getSolution(TypeID::dcdtSta, solution);
             double cdtVar = getVariance(TypeID::dcdtSta, covMatrix);

             // now, let's compute updated cdt
             double cdtNow;
             double cdtNowVar;

             try
             {
                 double cdtDelta = getSolution(TypeID::dcdtStaDelta, solution);
                 double cdtMinus;
                 cdtMinus = lastCdt + cdtDelta;
                 double cdtMinusVar = lastCdtVar;

                 // update var
                 cdtNowVar = 1.0/(1.0/cdtMinusVar + 1.0/cdtVar); 

                 // update solutions
                 cdtNow = cdtNowVar*(1.0/cdtMinusVar*cdtMinus + 1.0/cdtVar*cdt);

             }
             catch(InvalidRequest& e)
             {
                 cdtNowVar = cdtVar;
                 cdtNow = cdt;
             }

             lastCdt = cdt;
             lastCdtVar = cdtVar;

             double trop = getSolution(TypeID::wetMap, solution);

             // print forward float solutions
             if(pFloatSolStream!=NULL)
                printSols(pFloatSolStream, epoch, cdtNow, trop, numSat);

             // store the forward rinex data
             forwardRinexData[epoch] = gBack;
             forwardStateData[epoch] = solution;
             forwardCovData[epoch] = covMatrix;
         }


      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverDelta::postCompute()'

   void SolverDelta::PostProcess()
       noexcept(false)
   {
       try
       {
           // now, backward processing
           forwardProcessing=false;

           VariableDataMap allNwData;

           cout << "pGPSNw12Filter" << endl;

           VariableDataMap gpsNw12Data;
           if(pGPSNw12Filter!=NULL)
           {
               gpsNw12Data = pGPSNw12Filter->getAllFixedAmbData();
           }

           allNwData.insert(gpsNw12Data.begin(), gpsNw12Data.end());

           cout << "pGPSNw25Filter" << endl;

           VariableDataMap gpsNw25Data;
           if(pGPSNw25Filter!=NULL)
           {
               gpsNw25Data = pGPSNw25Filter->getAllFixedAmbData();
           }
           allNwData.insert(gpsNw25Data.begin(), gpsNw25Data.end());

           if(debug)
           {
                for(auto nd: allNwData)
                {
                    cout << nd.first << " :" << nd.second << endl;;
                }
           }

           cout << "Start processing ...." << endl;

           // loop all saved data in forward processing
           for(epochGnssRinexMap::reverse_iterator reverseIt = forwardRinexData.rbegin();
               reverseIt != forwardRinexData.rend();
               ++reverseIt)
           {
               CommonTime epoch( (*reverseIt).first );

               gnssRinex gRin( (*reverseIt).second );

               // Backward filtering
               Process(gRin);

               cout << "after Process" << endl;

               int numSat = gRin.numSats();

               // let's update the cdt using last epoch's cdt and cdtDelta
               // and current epoch's cdt
               double cdt    = getSolution(TypeID::dcdtSta, solution);
               double cdtVar = getVariance(TypeID::dcdtSta, covMatrix);

               // now, let's compute updated cdt
               double cdtNow;
               double cdtNowVar;

               // update var
               cdtNowVar = 1.0/(1.0/lastCdtVar+ 1.0/cdtVar); 

               // update solutions
               cdtNow = cdtNowVar*(1.0/lastCdtVar*lastCdt+ 1.0/cdtVar*cdt);

               try
               {
                   double cdtDelta = getSolution(TypeID::dcdtStaDelta, solution);

                   lastCdt = cdtNow - cdtDelta;
                   lastCdtVar = cdtNowVar;
               }
               catch(InvalidRequest& e)
               {
                   lastCdt = cdtNow;
                   lastCdtVar = cdtNowVar;
               }

               double trop = getSolution(TypeID::wetMap, solution);

               // print smoothed-float solutions
               if(pFloatSolStreamSmoothed!=NULL)
                   printSols(pFloatSolStreamSmoothed, epoch, cdtNow, trop, numSat);
  
               VariableDataMap fixedAmbData;
               VariableDataMap BUCData;
               VariableDataMap BUCVarData;

               if(pGPSNw12Filter!=NULL || pGPSNw25Filter!=NULL)
               {
                   getUCAmb(allNwData, solution, covMatrix, BUCData, BUCVarData);

                   // now, printout
                   if(pBL1StreamSmoothed!=NULL)
                       printAmbs(pBL1StreamSmoothed, epoch, BUCData, BUCVarData);
               }
  
           } // end of 'for(epoch)'


           // at last free the random
           forwardRinexData.clear();
           forwardStateData.clear();
           forwardCovData.clear();

       }
       catch(Exception& u)
       {
             // Throw an exception if something unexpected happens
          ProcessingException e( getClassName() + ":"
                                 + StringUtils::asString( getIndex() ) + ":"
                                 + u.what() );

          GPSTK_THROW(e);

       }

       return;
   }

   int SolverDelta::getUCAmb( VariableDataMap& NwData,
                              Vector<double>&  solution, 
                              Matrix<double>&  covMatrix,
                              VariableDataMap& BUCData,
                              VariableDataMap& BUCVarData)
   {


       // according to currentUnkSet, 
       // now, extract ambiguity-related var's state/cov for ambiguty input/outupt
       VariableSet ambVarSet;
       for(auto v : currentUnkSet )
       {
           SatID sat = v.getSatellite();
           if( v.getArcIndexed() )
           {
               ambVarSet.insert( v );
           }
       }

       for(auto v: ambVarSet )
       {
           if(NwData.find(v)!=NwData.end())
           {
               double BLC = solution(v.getNowIndex());
               double Nw  = NwData[v];

               ////////////////////
               //  BLC = B1 + f2/(f1-f2)*Nw;
               ////////////////////
               SatelliteSystem::Systems satSys = v.getSatSys();
               TypeID ambType = v.getType();

               if( satSys == SatelliteSystem::GPS && 
                   ambType == TypeID::BLC12)
               {
                    double f1  = L1_FREQ_GPS;
                    double f2  = L2_FREQ_GPS;
                    double BL1 = BLC - f2/(f1-f2)*Nw;
                    BUCData[v] = BL1;
                    int i = v.getNowIndex();
                    BUCVarData[v] = covMatrix(i,i);
               }
/*
               else if( satSys == SatelliteSystem::GPS && 
                        ambType == TypeID::BLC25)
               {
                    double f2  = L2_FREQ_GPS;
                    double f5  = L5_FREQ_GPS;
                    double BL2 = BLC - f5/(f2-f5)*Nw;
                    BUCData[v] = BL2;
                    int i = v.getNowIndex();
                    BUCVarData[v] = covMatrix(i,i);
               }
*/
           }
       }
   };


   Variable SolverDelta::getVariable( const TypeID& type, 
                               const VariableSet& varSet) const
      noexcept(false)
   {

         // Declare an iterator for 'stateMap' and go to the first element
      VariableSet::const_iterator it = varSet.begin();

         // Look for a variable with the same type
      while( (*it).getType() != type &&
             it != varSet.end() )
      {
         ++it;

         // If the same type is not found, throw an exception
         if( it == varSet.end() )
         {
             InvalidRequest e("Type not found in varSet.");
             GPSTK_THROW(e);
         }
      }

         // Else, return the corresponding value
      return (*it);

   }  // End of method 'SolverGeneral::getSolution()'


      /* Returns the solution associated to a given TypeID.
       *
       * @param type    TypeID of the solution we are looking for.
       *
       * \warning In the case the solution contains more than one variable
       * of this type, only the first one will be returned.
       */
   double SolverDelta::getSolution( const TypeID& type, 
                                  const Vector<double>& stateVec ) const
      noexcept(false)
   {
         // Declare an varIterator for 'stateMap' and go to the first element
      auto varIt = currentUnkSet.begin();
      while( (*varIt).getType() != type )
      {
         // If the same type is not found, throw an exception
         if( varIt == currentUnkSet.end() )
         {
             InvalidRequest e("Type not found in solution vector.");
             GPSTK_THROW(e);
         }

         varIt++;
      }

         // Else, return the corresponding value
      return stateVec( (*varIt).getNowIndex() );

   }  // End of method 'SolverGeneral::getSolution()'

   double SolverDelta::getVariance( const TypeID& type, 
                                    const Matrix<double>& covMatrix) const
      noexcept(false)
   {
         // Declare an varIterator for 'stateMap' and go to the first element
      auto varIt = currentUnkSet.begin();
      while( (*varIt).getType() != type )
      {
         // If the same type is not found, throw an exception
         if( varIt == currentUnkSet.end() )
         {
             InvalidRequest e("Type not found in solution vector.");
             GPSTK_THROW(e);
         }

         varIt++;
      }

         // Else, return the corresponding value
      return covMatrix( (*varIt).getNowIndex(), (*varIt).getNowIndex());

   }  // End of method 'SolverGeneral::getSolution()'


   // print float solutions
   void SolverDelta::printSols( std::ofstream* outStr,
                              const CommonTime& epoch, 
                              double cdtNow,
                              double trop,
                              int numSat)
   {
       YDSTime time( epoch );

          // print time
       *outStr << time.year << " "
               << time.doy  << " "
               << time.sod  << " ";

       *outStr << numSat << " ";

       *outStr << cdtNow << " "
               << trop << " "
               ;
       
       *outStr << endl;
   }


   // print float solutions
   void SolverDelta::printAmbs( std::ofstream* outStr,
                              const CommonTime& epoch, 
                              VariableDataMap& BUCMap,
                              VariableDataMap& BUCVarMap)
   {
       for(auto amb: BUCMap)
       {
           SourceID source = amb.first.getSource();
           SatID sat       = amb.first.getSatellite();
           double satArc   = amb.first.getArc();
           TypeID type     = amb.first.getType();
           double value    = amb.second;
           double variance = BUCVarMap[amb.first];
           double frac     = value - std::floor(value+0.5);

              // print time
           *outStr << YDSTime(epoch).printf("%Y %03j %14.3s") << " ";
           *outStr << source << " "
                   << sat << " "
                   << rightJustify(asString<int>(int(satArc)),6) << " "
                   << type << " "
                   << rightJustify(asString(value,6),13) << " "
                   << rightJustify(asString(variance,6),13) << " "
                   << rightJustify(asString(frac,6),13) << " "
                   << endl;
       }

   };


}  // End of namespace gpstk
